#!/usr/bin/python
# -*- coding: UTF-8 -*-
import time

class Bipe4TRobot:
	TIME=0.6
	TIMES=0.8      #滑步0.6 转向、跳舞0.8
	_ids = [0]*8

########速度#######
	SPEED = 30
	SPEED2 = [SPEED,SPEED]
	SPEED4 = [SPEED,SPEED,SPEED,SPEED]
	SPEED6 = [SPEED,SPEED,SPEED,SPEED,SPEED,SPEED]
	SPEED8 = [SPEED,SPEED,SPEED,SPEED,SPEED,SPEED,SPEED,SPEED]


########协议传感器开关#######
	SENSOR_ON = [1]
	SENSOR_OFF = [0]

########同时控制整条腿（3个模块）########
	# legL = [ 36,37,38,39 ]  #面向无博文标签面左边
	# legR = [ 30,31,32,33 ]  #面向无博文标签面右边
	legL = []  #面向无博文标签面左边
	legR = []  #面向无博文标签面右边

########同时控制两条腿的髌与膝盖########
	# legTwoHK = [ 31,32,37,38 ]
	legTwoHK = []
########滑步########
	# legSlideR = [ 30,33 ]
	# legSlideL = [ 36,39 ]
	# legSlideAll = [ 30,33,36,39 ]
	legSlideR = [ ]
	legSlideL = [ ]
	legSlideAll = [  ]

########跳舞########
	# legBow  = [ 31,37 ]
	# legStand = [ 31,32,33,37,38,39 ]
	# legOnTiptoe = [ 33,39 ]
	# legAll = [  30,31,32,33, 36,37,38,39  ]
	legBow  = [  ]
	legStand = [  ]
	legOnTiptoe = [  ]
	legAll = [   ]

	Step_TurnInit = [0,0,0,0]

########前进########
	HIP_STEP =15  #蓝色的模块能用    蓝色科研版每个脚加4个舵机用18
	# HIP_STEP =12  ##科研版参数
	KNEE_STEP =15
	ANKLE_STEP =15
	FORWARD_UP_STEPS = [0, 0, 0, -HIP_STEP] 
	FORWARD_RIGHT_STEPS = [-KNEE_STEP,ANKLE_STEP, -KNEE_STEP,ANKLE_STEP ] 
	FORWARD_LEFT_STEPS = [KNEE_STEP,-ANKLE_STEP, KNEE_STEP,-ANKLE_STEP ] 

########后退########
	BACK_RIGHT_STEPS = [KNEE_STEP,-ANKLE_STEP, KNEE_STEP,-ANKLE_STEP]
	BACK_LEFT_STEPS = [ -KNEE_STEP,ANKLE_STEP, -KNEE_STEP,ANKLE_STEP ]

########转向步态########
	KNEE_TURN = 45
	ANKLE_TURN =45

########原地左转########
	TURN_LEFT_STEPS = [ -KNEE_TURN,ANKLE_TURN,-KNEE_TURN,ANKLE_TURN ]

########原地右转########
	TURN_RIGHT_STEPS = [ KNEE_TURN,-ANKLE_TURN,KNEE_TURN,-ANKLE_TURN ]
	

########滑步状态########
##模仿大模块步态需要改成 knee 70  ankle 20 ankleUp 30 
##滑步步态需改成knee 45 ankle 30   延时500毫秒  由于机器人刚度不足，以此两个角度加起来等于90度效果不好
	KNEE_STEP_SLIDE =45
	ANKLE_STEP_SLIDE=30
	ANKLE_STEP_SLIDEUP =30
	SLIDE_UP_STEPS = [-KNEE_STEP_SLIDE,ANKLE_STEP_SLIDE ]
	SLIDE_DOWN_STEPS = [-90,0]
	SLIDE_INIT_STEPS = [-90,0,-90,0]

########跳舞   延时要800毫秒########
	DANCE_HIP_STEP =70
	DANCE_ANKLE_STEP =25
	DANCE_BOW_DOWN_STEPS = [ -DANCE_HIP_STEP,DANCE_HIP_STEP ]
	DANCE_BOW_INIT = [ 0,0 ]
	DANCE_STAND_UP = [ 0,0,DANCE_ANKLE_STEP,0,0,DANCE_ANKLE_STEP ]
	DANCE_STAND_DOWN = [ -50,50,0,50,-50,0 ]
	DANCE_STAND_INIT = [ 0,0,0,0,0,0 ]
	DANCE_SHAKE = [-70,30]
	DANCE_SHAKE_INIT = [ -90,0 ]

	DANCE_IN = [ 15 ,15 ]
	DANCE_OUT = [ -15 ,-15 ]

#########RGB彩灯
	SENSOR_RGB = [[255,1,1],[1,255,1],[1,1,255],[255,255,1],[255,1,255],[1,255,255],[255,255,255],[255,100,1],[1,1,1]]

	scanned_ids=[]
	init_position=[]
	moving_count = 0


	#初始化机器人：使用 BwRobotLib 对象
	def __init__(self,robotlib):
		self.robotlib = robotlib
		self.lib = self.robotlib.mclib

    #绑定ID： T T IU (F) T T IU
	def bindIds(self,ids):
		self._ids = ids
		self.legR = [ self._ids[0],self._ids[1],self._ids[2],self._ids[3] ]
		self.legL = [ self._ids[4],self._ids[5],self._ids[6],self._ids[7] ]
		self.legTwoHK = [ self._ids[1],self._ids[2],self._ids[5],self._ids[6] ]
		self.legSlideR = [ self._ids[0],self._ids[3] ]
		self.legSlideL = [ self._ids[4],self._ids[7] ]
		self.legSlideAll = [ self._ids[0],self._ids[3],self._ids[4],self._ids[7] ]
		self.legBow = [ self._ids[1],self._ids[5] ]
		self.legStand = [ self._ids[1],self._ids[2],self._ids[3],self._ids[5],self._ids[6],self._ids[7] ]
		self.legOnTiptoe = [ self._ids[3],self._ids[7] ]
		self.legAll = [ self._ids[0],self._ids[1],self._ids[2],self._ids[3],self._ids[4],self._ids[5],self._ids[6],self._ids[7] ]
		#self.scanned_ids = self.lib.ScanId(0,50)
		#print(self.scanned_ids[0]) 
		for a in range(0,8):
			self.lib.SetMotorMode(self._ids[a],0)
		self.init_position = [ -90,0,0,0,-90,0,0,0]
		res=self.lib.SetMotorPositionSyncWithSpeeds(self._ids,self.init_position,self.SPEED8)
		time.sleep(self.TIME)
		if(res > 0):
			print("机器人初始化设置成功")
		else:
			print("机器人初始化设置失败")


   ######################################################################
   ############################# 复杂双足模式 ###########################
   ######################################################################
	###快速初始化
	def quickInit(self):
		self.lib.SetMotorPositionSyncWithSpeeds(self._ids,self.init_position, self.SPEED8)
		time.sleep(self.TIME)

	###前进步态
	def goForward(self):
		self.lib.SetPositionWithSpeed(self.legL[3],-self.HIP_STEP ,self.SPEED)
		time.sleep(0.7)
		self.lib.SetMotorPositionSyncWithSpeeds(self.legTwoHK, self.FORWARD_RIGHT_STEPS, self.SPEED4)
		time.sleep(0.4)
		self.lib.SetPositionWithSpeed(self.legL[3], 0 ,self.SPEED)
		time.sleep(0.7)
		self.lib.SetPositionWithSpeed(self.legR[3], -self.HIP_STEP ,self.SPEED)#这里需要比左腿转得小，机械结构问题
		time.sleep(0.7)
		self.lib.SetMotorPositionSyncWithSpeeds(self.legTwoHK, self.FORWARD_LEFT_STEPS, self.SPEED4)
		time.sleep(0.4)
		self.lib.SetPositionWithSpeed(self.legR[3], 0 ,self.SPEED)
		time.sleep(0.7)

	###后退步态
	def goBack(self):
		self.lib.SetPositionWithSpeed(self.legL[3], -self.HIP_STEP ,self.SPEED)
		time.sleep(self.TIME)
		self.lib.SetMotorPositionSyncWithSpeeds(self.legTwoHK, self.BACK_RIGHT_STEPS, self.SPEED4)
		time.sleep(0.4)
		self.lib.SetPositionWithSpeed(self.legL[3], 0 ,self.SPEED)
		time.sleep(self.TIME)
		self.lib.SetPositionWithSpeed(self.legR[3], -self.HIP_STEP ,self.SPEED)#这里需要比左腿转得小，机械结构问题
		time.sleep(self.TIME)
		self.lib.SetMotorPositionSyncWithSpeeds(self.legTwoHK, self.BACK_LEFT_STEPS, self.SPEED4) 
		time.sleep(0.4)
		self.lib.SetPositionWithSpeed(self.legR[3], 0 ,self.SPEED)
		time.sleep(self.TIME)

	###原地向左
	def turnLeft(self):
		self.lib.SetMotorPositionSyncWithSpeeds(self.legTwoHK, self.Step_TurnInit, self.SPEED4)
		time.sleep(self.TIMES)
		self.lib.SetMotorPositionSyncWithSpeeds(self.legTwoHK, self.TURN_LEFT_STEPS, self.SPEED4)
		time.sleep(self.TIMES)

	###y原地右转
	def turnRight(self):
		self.lib.SetMotorPositionSyncWithSpeeds(self.legTwoHK, self.Step_TurnInit, self.SPEED4)
		time.sleep(self.TIMES)
		self.lib.SetMotorPositionSyncWithSpeeds(self.legTwoHK, self.TURN_RIGHT_STEPS, self.SPEED4)
		time.sleep(self.TIMES)

	###向左滑步
	def slideRight(self):
		self.lib.SetMotorPositionSyncWithSpeeds(self.legSlideR, self.SLIDE_UP_STEPS, self.SPEED2) 
		time.sleep(self.TIME)
		self.lib.SetPositionWithSpeed(self.legL[3], -15 ,self.SPEED)
		time.sleep(self.TIME)
		self.lib.SetMotorPositionSyncWithSpeeds(self.legSlideR, self.SLIDE_DOWN_STEPS,self.SPEED2)
		time.sleep(self.TIME)
		self.lib.SetMotorPositionSyncWithSpeeds(self.legAll,self.init_position,self.SPEED8)
		# self.lib.SetPosition(self.legL[3], 0)
		time.sleep(self.TIME)

	###向右滑步
	def slideLeft(self):
		self.lib.SetMotorPositionSyncWithSpeeds(self.legSlideL, self.SLIDE_UP_STEPS,self.SPEED2) 
		time.sleep(self.TIME)
		self.lib.SetPositionWithSpeed(self.legR[3], -15 ,self.SPEED)
		time.sleep(self.TIME)
		self.lib.SetMotorPositionSyncWithSpeeds(self.legSlideL, self.SLIDE_DOWN_STEPS,self.SPEED2)
		time.sleep(self.TIME)
		self.lib.SetMotorPositionSyncWithSpeeds(self.legAll,self.init_position,self.SPEED8)
		# self.lib.SetPosition(self.legR[3], 0)
		time.sleep(self.TIME)

	###跳舞   
	def dance(self):
	###鞠躬
		self.lib.SetSensorData(8,0x02,self.SENSOR_RGB[0])
		for Count1 in range(0,3):
			self.lib.SetMotorPositionSyncWithSpeeds(self.legBow, self.DANCE_BOW_DOWN_STEPS,self.SPEED2)
			time.sleep(self.TIMES)
			self.lib.SetMotorPositionSyncWithSpeeds(self.legBow, self.DANCE_BOW_INIT,self.SPEED2) 
			time.sleep(self.TIMES)
			print ('鞠躬')
	###蹲下加垫脚尖
		self.lib.SetSensorData(8,0x02,self.SENSOR_RGB[1])
		for Count2 in range(0,3):
			self.lib.SetMotorPositionSyncWithSpeeds(self.legStand, self.DANCE_STAND_UP,self.SPEED6)
			time.sleep(self.TIMES)
			self.lib.SetMotorPositionSyncWithSpeeds(self.legStand, self.DANCE_STAND_DOWN,self.SPEED6) 
			time.sleep(self.TIMES)
			print ('蹲下加垫脚尖')
		self.lib.SetMotorPositionSyncWithSpeeds(self.legStand, self.DANCE_STAND_INIT,self.SPEED6) 
		time.sleep(self.TIMES)
	###左右摇摆
		self.lib.SetSensorData(8,0x02,self.SENSOR_RGB[2])
		self.lib.SetPositionWithSpeed(self.legR[3], -20 ,self.SPEED)
		time.sleep(self.TIMES)
		for Count3 in range(0,3):
			self.lib.SetMotorPositionSyncWithSpeeds(self.legSlideL, self.DANCE_SHAKE,self.SPEED2)
			time.sleep(self.TIMES)
			if(Count3==2):
				self.lib.SetPositionWithSpeed(self.legR[3], 0 ,self.SPEED)
			self.lib.SetMotorPositionSyncWithSpeeds(self.legSlideL, self.DANCE_SHAKE_INIT,self.SPEED2) 
			time.sleep(self.TIMES)
			print ('摆左腿')
		self.lib.SetPositionWithSpeed(self.legL[3], -20 ,self.SPEED)
		time.sleep(self.TIMES)
		for Count4 in range(0,3):
			self.lib.SetMotorPositionSyncWithSpeeds(self.legSlideR, self.DANCE_SHAKE,self.SPEED2) 
			time.sleep(self.TIMES)
			if (Count4 == 2):
				self.lib.SetPositionWithSpeed(self.legL[3], 0 ,self.SPEED)
			self.lib.SetMotorPositionSyncWithSpeeds(self.legSlideR, self.DANCE_SHAKE_INIT,self.SPEED2) 
			time.sleep(self.TIMES)
			print ('摆右腿')
	###垫脚尖
		self.lib.SetSensorData(8,0x02,self.SENSOR_RGB[3])
		for Count5 in range(0,3):
			self.lib.SetMotorPositionSyncWithSpeeds(self.legOnTiptoe, self.DANCE_IN,self.SPEED2)
			time.sleep(self.TIMES)
			self.lib.SetMotorPositionSyncWithSpeeds(self.legOnTiptoe, self.DANCE_OUT,self.SPEED2)
			time.sleep(self.TIMES)
			print ('垫脚尖')
		self.lib.SetMotorPositionSyncWithSpeeds(self.legOnTiptoe, self.DANCE_BOW_INIT,self.SPEED2)
		time.sleep(self.TIMES)
		self.lib.SetSensorData(8,0x02,self.SENSOR_RGB[8])

	###前进、后退、左移、右移各两个循环，左转、右转５个循坏   
	def runDemo(self):
		self.lib.SetSensorData(8,0x02,self.SENSOR_RGB[2])
		for self.moving_count in range(0,2):
			print('向前走')
			self.goForward()
		self.quickInit()
		time.sleep(self.TIMES)
		self.lib.SetSensorData(8,0x02,self.SENSOR_RGB[3])
		for self.moving_count in range(0,2):
			print('向后走')
			self.goBack()
		self.quickInit()
		time.sleep(self.TIMES)
		self.lib.SetSensorData(8,0x02,self.SENSOR_RGB[4])
		for self.moving_count in range(0,3):
			print('向左走')
			self.slideLeft()
		self.quickInit()
		time.sleep(self.TIMES)
		self.lib.SetSensorData(8,0x02,self.SENSOR_RGB[5])
		for self.moving_count in range(0,3):
			print('向右走')
			self.slideRight()
		self.quickInit()
		time.sleep(self.TIMES)
		self.lib.SetSensorData(8,0x02,self.SENSOR_RGB[1])
		for self.moving_count in range(0,8):
			print('左转')
			self.turnLeft()
		self.quickInit()
		time.sleep(self.TIMES)
		self.lib.SetSensorData(8,0x02,self.SENSOR_RGB[7])
		for self.moving_count in range(0,10):
			print('右转')
			self.turnRight()
		self.quickInit()
		self.lib.SetSensorData(8,0x02,self.SENSOR_RGB[8])

    ##############手势控制机器人
	def runGestureRemoteRobot(self):
		count = 2
		count1 = 8
		delay = 0.5
		while 1:
			print("请输入手势:")
			s3 = self.lib.SetSensorData(12,0x01,self.SensorOn)
			s1 = self.lib.GetSensorData(12,0x01)
			print ("返回值：",s1)
			if(s1==1):#从下到上
				print("正常活动")
				self.runDemo()
			if(s1==2):#从上到下
				print("跳舞")
				self.dance()
				self.quickInit()

